#include "servo.h"
#include "led.h"
#include "delay.h"
#include "pstwo.h" 

//PWM输出初始化
//arr：自动重装值
//psc：时钟预分频数
extern u16 System_Time_Counter;
unsigned char PWM1Control=1;
unsigned char Flag_ServoAdjust=0;
unsigned char Flag_ServoLock=0;
unsigned char PWM1Reverse=0;
unsigned char Servo_ControlMode=0;
//servo数据数组指针0,1分别对应左右和俯仰。
//下面代表了舵机输出PWM的极限和默认值。
int ServoPWM_Max[Servo_TotalNumber]={2250,2250};
int ServoPWM_Min[Servo_TotalNumber]={450,450};
int ServoPWM_Default[Servo_TotalNumber]={1350,1350};
u16 ServoPWM_Out[Servo_TotalNumber];
//先计算舵机转角，再转换成PWM输出。
int ServoAngle_Max[Servo_TotalNumber]={180,135};
int ServoAngle_Default[Servo_TotalNumber]={90,90};
int ServoAngle_Min[Servo_TotalNumber]={0,45};
float PS2_ServoPosition[Servo_TotalNumber];//细化之后的PS2位置数组，0-1800；
float SpeedRatio[5]={0.0,0.25,1.0,3.0,20.0};//速度比率，通过全局时钟实现舵机的类匀速转动

int Rate_FromAngleToPWM=10;//从角度到PWM的换算比率，同时决定了舵机的角度分辨率（1度除以该值）
u8 Servo_AdjustRoutine=10;//舵机角度调整周期，由全局时钟满足该值（ms）的倍数时调整舵机角度，实现类匀速运动。

void Servo_PWM_init(u16 arr,u16 psc)
{ 
	int i;
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);// 
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);  //使能GPIO外设时钟使能
	                                                                     	
   //设置该引脚为复用输出功能,输出TIM1 CH1的PWM脉冲波形
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11; //
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值  18000
	//最后介于0-180角度控制范围PWM取值为450-2250
	TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:80 72000000/（18000*80）=50HZ
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性高
	TIM_OC1Init(TIM1, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
	TIM_OC4Init(TIM1, &TIM_OCInitStructure);

  	TIM_CtrlPWMOutputs(TIM1,ENABLE);	//MOE 主输出使能	
	 
	
	TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器
	for(i=0;i<Servo_TotalNumber;i++)
	{
		ServoPWM_Out[i]=ServoPWM_Default[i];
	}
	TIM_Cmd(TIM1, ENABLE);  //使能TIM1
	TIM_SetCompare1(TIM1,ServoPWM_Out[0]);
	TIM_SetCompare4(TIM1,ServoPWM_Out[1]);

//下面是给切换舵机控制通道用的继电器初始化用的信号端初始化
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);

    GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7|GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
    GPIO_Init(GPIOB,&GPIO_InitStructure);

    GPIO_ResetBits(GPIOB,GPIO_Pin_7|GPIO_Pin_8);
}

void Servo_PositionMode_Calc(void)
{
	int i;
	float ConversionRate[2]={1800.0,1800.0};
	PS2_ServoPosition[0]=(255.0-(float)PS2_AnologData(PSS_RX))/255.0*ConversionRate[0];
	PS2_ServoPosition[1]=(255.0-(float)PS2_AnologData(PSS_RY))/255.0*ConversionRate[1];
	for (i=0;i<Servo_TotalNumber;i++)
	{
		if(PS2_ServoPosition[i]<ServoAngle_Min[i]*Rate_FromAngleToPWM)
		{
			PS2_ServoPosition[i]=ServoAngle_Min[i]*Rate_FromAngleToPWM;
		}
		else if(PS2_ServoPosition[i]>ServoAngle_Max[i]*Rate_FromAngleToPWM)
		{
			PS2_ServoPosition[i]=ServoAngle_Max[i]*Rate_FromAngleToPWM;
		}
		ServoPWM_Out[i]=PS2_ServoPosition[i]+ServoPWM_Min[i];
	}
	
}
void Servo_SpeedMode_Calc(void)
{
	int i;
	
	int Servo_Speed[Servo_TotalNumber];
	int Servo_Direction[Servo_TotalNumber];
	//为了实现摇杆和舵机转动同向，以向左向上Direction=1，向右向下direction=-1，调节两个变量同向
	int PS2_DefaultPosition[2]={128,127};//摇杆回位默认值，根据摇杆的初始值校准
	int PS2_ServoOffset[Servo_TotalNumber];

	PS2_ServoOffset[0]=(float)PS2_AnologData(PSS_RX)-PS2_DefaultPosition[0];
	PS2_ServoOffset[1]=(float)PS2_AnologData(PSS_RY)-PS2_DefaultPosition[1];
	for (i=0;i<Servo_TotalNumber;i++)
	{
		if(PS2_ServoOffset[i]<0)
		{
			PS2_ServoOffset[i]*=-1;
			Servo_Direction[i]=1;
		}
		else
		{
			Servo_Direction[i]=-1;
		}

		if(PS2_ServoOffset[i]<5)
		{
			Servo_Speed[i]=0;
		}
		else if(PS2_ServoOffset[i]<32)
		{
			Servo_Speed[i]=1;
		}
		else if(PS2_ServoOffset[i]<32*2)
		{
			Servo_Speed[i]=2;
		}
		else if(PS2_ServoOffset[i]<32*3)
		{
			Servo_Speed[i]=3;
		}
		else
		{
			Servo_Speed[i]=4;
		}
	}
	if(Flag_ServoAdjust)
	{
		for(i=0;i<Servo_TotalNumber;i++)
		{
			PS2_ServoPosition[i]+=(SpeedRatio[Servo_Speed[i]]*Servo_Direction[i]);
			if(PS2_ServoPosition[i]<ServoAngle_Min[i]*Rate_FromAngleToPWM)
			{
				PS2_ServoPosition[i]=ServoAngle_Min[i]*Rate_FromAngleToPWM;
			}
			else if(PS2_ServoPosition[i]>ServoAngle_Max[i]*Rate_FromAngleToPWM)
			{
				PS2_ServoPosition[i]=ServoAngle_Max[i]*Rate_FromAngleToPWM;
			}
			ServoPWM_Out[i]=PS2_ServoPosition[i]+ServoPWM_Min[i];
		}
		
		Flag_ServoAdjust=0;
	}

}
void Servo_Control(void)
{
	int i;
	if(Flag_ServoLock||Flag_Disconnect)//如果舵机锁定或者ps2手柄断联
	{
		return;
	}
	switch (Servo_ControlMode)//模式选择：实时视角模式和偏移模式
	{
	case 0:
		Servo_PositionMode_Calc();
		break;
	case 1:
		Servo_SpeedMode_Calc();
	default:
		break;
	}
	for(i=0;i<Servo_TotalNumber;i++)//输出前的输出值校验
	{
		if(ServoPWM_Out[i]>ServoPWM_Max[i])
		{
			ServoPWM_Out[i]=ServoPWM_Max[i];
		}
		if(ServoPWM_Out[i]<ServoPWM_Min[i])
		{
			ServoPWM_Out[i]=ServoPWM_Min[i];
		}
	}
	TIM_SetCompare1(TIM1,ServoPWM_Out[0]);
	TIM_SetCompare4(TIM1,ServoPWM_Out[1]);
}
void Servo_Reset(void)
{
	int i;
	for(i=0;i<Servo_TotalNumber;i++)
	{
		PS2_ServoPosition[i]=ServoAngle_Default[i]*Rate_FromAngleToPWM;
		ServoPWM_Out[i]=ServoPWM_Default[i];
	}
	TIM_SetCompare1(TIM1,ServoPWM_Out[0]);
	TIM_SetCompare4(TIM1,ServoPWM_Out[1]);
}
